import os
import os.path as osp
import math
import numpy as np


def change_angle(file_path):
    lines = [f.strip("\n") for f in open(file_path, "r").readlines()]
    lines = [line.split(' ')[:15] for line in lines]
    for line in lines:
        angle = float(line[-1])
        if angle > np.pi:
            print("old angle: " + str(angle))
            angle = angle - 2 * np.pi
            print("new angle: " + str(angle))
        elif angle < -np.pi:
            print("old angle: " + str(angle))
            angle = 2 * np.pi + angle
            print("new angle: " + str(angle))
        line[-1] = str(angle)
    return lines

def main(root_dir, out_dir):
    if not osp.exists(out_dir):
        os.makedirs(out_dir)
    files = os.listdir(root_dir)
    for f in files:
        file_path = osp.join(root_dir, f)
        out_path = osp.join(out_dir, f)
        lines = change_angle(file_path)
        # import pdb
        # pdb.set_trace()
        for line in lines:
            final_line = ' '.join(line)
            with open(out_path, "a+") as f:
                f.write(final_line + "\n")



if __name__ == "__main__":
    root_dir = "data/ww/training/label_2"
    out_dir = "data/ww/label_2_angle_modified"
    main(root_dir, out_dir)
